#include "MathUtil.h"
#include "defs.h"

#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_pow_int.h>
#include <iostream>
#include <gsl/gsl_math.h>

#include <QtGui/QApplication>
#include <QtGui/QDesktopWidget>


namespace jcrada {

    double Math::scaleParma( double src_min, double src_max, double value, double target_min, double target_max ) {
        //Cortesía de Rubén Parma :)
        return (target_max - target_min) / (src_max - src_min) * (value - src_min) + target_min;
    }

    double Math::scale( double src_min, double src_max, double src_val,
            double target_min, double target_max ) {
        assert(src_val >= src_min && src_val <= src_max);
        //        double translate = -src_min;
        //        src_min += translate;
        //        src_max += translate;
        //        src_val += translate;
        //        double position = src_val / (src_min + src_max);
        //        translate = -target_min;
        //        target_min += translate;
        //        target_max += translate;
        //        src_val += translate;
        //        double result = ((target_min + target_max) * position) - translate;
        //        assert(result >= target_min && result <= target_max);
        //        return result;
        return scaleParma(src_min, src_max, src_val, target_min, target_max);
        //        DEBUG("src_min=" + TO_STRING(src_min) + "; src_max=" + TO_STRING(src_max)  +
        //                "; src_val=" + TO_STRING(src_val) + "; target_min=" + TO_STRING(target_min) +
        //                "; target-max=" + TO_STRING(target_max) + "---" + TO_STRING(result));
    }

    double Math::abs( double value ) {
        return value < 0 ? -value : value;
    }

    double Math::sigmoid( double value ) {
        return 1 / (1 + exp(-value));
    }

    void Math::copy( int size, const double * src, double * target ) {
        for (int i = 0; i < size; i++) {
            target[i] = src[i];
        }
    }

    long Math::modEuclidean( long D, long d ) {
        long r = D % d;
        if (r < 0) {
            if (d > 0) {
                r = r + d;
            } else {
                r = r - d;
            }
        }
        return r;
    }

    long Math::modFloor( long D, long d ) {
        long r = D % d;
        if ((r > 0 && d < 0) || (r < 0 && d > 0)) {
            r = r + d;
        }
        return r;
    }

    int Math::binToInt( double const * bin, int size ) {
        int pow = 0;
        int result = 0;
        for (int i = size - 1; i >= 0; --i) {
            result += bin[i] * gsl_pow_int(2, pow++);
        }
        return result;
    }

    void Math::fillBinaryInputs( double ** inputs, int input_size ) {
        for (int i = 0; i < input_size; ++i) {
            bool fill = true;
            for (int j = 0; j < gsl_pow_int(2.0, input_size); ++j) {
                if (j % (static_cast<int> (gsl_pow_int(2.0, input_size - i) / 2)) == 0) {
                    fill = !fill;
                }
                inputs[i][j] = fill ? 1.0 : 0.0;
            }
        }
    }

    double Math::degToRad( double degrees ) {
        return degrees * M_PI / 180.0;
    }

    double Math::radToDeg( double radians ) {
        return radians * 180.0 / M_PI;
    }

    double Math::getGradient( ePlane plane, const btVector3& a, const btVector3& b ) {
        switch (plane) {
            case XY: return (a.y() - b.y()) / (a.x() - b.x());
            case YZ: return (a.z() - b.z()) / (a.y() - b.y());
            case ZX: return (a.x() - b.x()) / (a.z() - b.z());
            default: assert(0);
        }
    }

    double Math::getAngleInRadians( ePlane plane, const btVector3& origin, const btVector3& endpoint ) {
        double result = 0.0;
        btVector3 magnitude = origin - endpoint;
        switch (plane) {
            case XY:
                result = atan2(magnitude.y(), magnitude.x());
                break;
            case YZ:
                result = atan2(magnitude.y(), magnitude.z());
                break;
            case ZX:
                result = atan2(magnitude.z(), magnitude.x());
                //                result *= (endpoint.x() < origin.x()) ? -1.0 : 1.0;
                break;
            default: assert(0);
        }
        return result;
    }

    double Math::getAngleInDegrees( ePlane plane, const btVector3& origin, const btVector3& endpoint ) {
        return radToDeg(getAngleInRadians(plane, origin, endpoint));
    }

    void Math::cartesianToSpherical( const btVector3& coord,
            double& radial, double& azimuth, double& zenith ) {
        radial = sqrt(gsl_pow_2(coord.x()) + gsl_pow_2(coord.y()) + gsl_pow_2(coord.z()));
        //        azimuth = radToDeg(atan2(sqrt(gsl_pow_2(coord.x()) + gsl_pow_2(coord.y())), coord.z()));
        azimuth = radToDeg(acos(coord.z() / radial));
        zenith = radToDeg(atan2(coord.y(), coord.x()));
    }

    void Math::centerInDesktop( const int width, const int height, int& pos_x, int& pos_y ) {
        pos_y = (QApplication::desktop()->height() / 2.0) - (height / 2.0);
        pos_x = (QApplication::desktop()->width() / 2.0) - (width / 2.0);
    }

    QRect Math::centerInDesktop( const QRect& rect ) {
        QRect result = rect;
        int x, y;
        Math::centerInDesktop(result.width(), result.height(), x, y);
        result.moveTo(x, y);
        return result;
    }

    int Math::test( int argc, char** argv ) {
        (void) argc;
        (void) argv;
        return 0;
        double angle = 180;
        double x, y;
        //        while (angle < 270) {
        //            x = cos(degToRad(angle));
        //            y = sin(degToRad(angle));
        //            //            std::cout << "angle: " << angle << " ; " << "atan(" << y << "/" << x << ") = " << radToDeg(atan(y / x)) << std::endl;
        //            std::cout << "angle: " << angle << " ; " << "atan2(" << y << "," << x << ") = " << radToDeg(atan2(y, x)) << std::endl;
        //            angle++;
        //        }

        //        return 0;

        double result = 0.0;
        double max = 100;
        for (float i = 0; i < 100; i += 4.99999999) {
            BDEBUG("scale(0, 100, " + TO_STRING(i) + ", 0, " + TO_STRING(max) + ") = " +
                    TO_STRING((int) scale(0, 100, i, 0, max)));
        }
        BDEBUG("scale(0, 100, " + TO_STRING(99.9999999) + ", 0, " + TO_STRING(max) + ") = " +
                TO_STRING((int) scale(0, 100, 99.99999999, 0, max)));

        return 0;
    }

}
